"""
===================================================
    FILENAME: algorithm.py
    东川路集群智能运输大队
    指导老师：张庆昊，邬明宇，卢俊国
    小组成员：景序，谭睿佳，于沛龙
===================================================
"""

from rflysim import RflysimEnvConfig, Vel
from rflysim.client import VehicleClient
from rflysim.config import AreaInfo,Position
import math
import time
import numpy as np
from objects import Pick, Delivery, Barrier, Vehicle, Shape, Line, Task, Package
import threading
import queue
import concurrent.futures

COLLAB_DIST = 15
COLLISION_THRESHOLD = 25.0  # 预警距离阈值（单位：米，可根据实际调整）
COLLSION_TEST_TIME = 0.1  # 预警测试时间间隔（单位：秒，可根据实际调整）

# 创建全局锁和任务队列
task_queue = queue.Queue()
task_lock = threading.Lock()
vehicle_status_lock = threading.Lock()
area_content_lock = threading.Lock()
# 新增：协同任务屏障全局字典
collab_barrier_dict = {}  # {task_id: threading.Barrier}
# 全局变量：车辆碰撞预警队列
collision_warning_queue = queue.Queue()

"""
currently only test for one vehicle
preparing for multi-vehicle

explanation for "state":
    developing: function to be finished
    bug fixing: already finished and may have potential bugs

          ------------- ABOVE: includes and declearation ------ BELOW: key algorithm functions ----------------    
"""


"""
state: bug fixing
"""
# extent the barrier for 5 meters
class ExtendedBarrier:
    def __init__(self, barrier:Barrier):
        self.origin = barrier
        self.ID = barrier.ID
        self.shape = barrier.shape
        self.shape.nZoneType = barrier.shape.nZoneType

        if barrier.shape.nZoneType == 1:
            self.shape.radius = barrier.shape.radius + 5
            self.shape.center = barrier.shape.center
            
        elif barrier.shape.nZoneType == 0 or barrier.shape.nZoneType == 3:
            """
            extend algorithm
            """
            vertices = barrier.shape.vertices
            extendedVertices = []
            
            # 遍历多边形的每个顶点
            n = len(vertices)
            for i in range(n):
                # 获取当前顶点及其相邻顶点
                prePoint = vertices[(i - 1) % n]   # 前一个顶点
                curPoint = vertices[i]            # 当前顶点（角点）
                subPoint = vertices[(i + 1) % n]   # 后一个顶点
                
                # 计算角平分线（Angle Bisector）上的扩展点（距离5米）
                exPoint = self._getAngleBisPoint(prePoint, curPoint, subPoint)
                extendedVertices.append(exPoint)
            
            self.shape.vertices = extendedVertices
            
            self.sides = [Line(self.shape.vertices[i], self.shape.vertices[i+1]) for i in range(len(self.shape.vertices)-1)]
            self.sides.append(Line(self.shape.vertices[-1], self.shape.vertices[0]))

    def getType(self):
        return "barrier_extended"

    def _getAngleBisPoint(self, p1:list[float, float], p2:list[float, float], p3:list[float, float]):
        prePoint = np.array(p1)
        curPoint = np.array(p2)
        subPoint = np.array(p3)
        
        # 计算从顶点到相邻点的向量
        vec1 = prePoint - curPoint
        vec2 = subPoint - curPoint
        x1, y1, x2, y2 = vec1.tolist()[0], vec1.tolist()[1], vec2.tolist()[0], vec2.tolist()[1]
        
        # 计算角平分线方向向量（bisector vector）
        biscVec = -(vec1/np.linalg.norm(vec1) + vec2/np.linalg.norm(vec2))

        # phi是角分线向量与经线的夹角
        sin_phi = biscVec.tolist()[0] / np.linalg.norm(biscVec)
        cos_phi = biscVec.tolist()[1] / np.linalg.norm(biscVec)
        cosA = (x1 * x2 + y1 * y2) / (np.linalg.norm(vec1) * np.linalg.norm(vec2))
        sinHalfA = math.sqrt((1 - cosA) / 2)
        l = 5.0 / sinHalfA

        pointA = meterToLatLon(l, p2, sin_phi, cos_phi)
        pointB = meterToLatLon(l, p2, -sin_phi, -cos_phi)

        return pointA if not isInsideArea(pointA, self.origin) else pointB


"""
state: bug fixing
"""
# after setting destination area, find out the best position to go
def setDestinationPos(vehicle:Vehicle, route1:list, route2:list, targetArea):
    if targetArea != 'home':
        endPosList = getRefinementPoint(targetArea, route1[-2], route2[1])
        endPos = Position(endPosList[0], endPosList[1])
    else:
        endPos = vehicle.startPos
    return endPos


"""
state: bug fixing
"""
def getRefinementPoint(rect, point_A:Position, point_B:Position) -> list:
    """
    Function:
        对取货路径做局部优化, 使得载具不需要走到Pick/Delivery的中心点取货
        返回优化后的路径节点, 节点位于
    Parameter:
        rect: Pick or Delivery
        pointA: 上一次A*的倒数第二个点坐标
        pointB: 下一次A*的第一个坐标
    """
    pointA = [point_A.x, point_A.y]
    pointB = [point_B.x, point_B.y]
    x_t, x_b, y_t, y_b = contractRect(rect)
    line_AB = Line(pointA, pointB)
    point_square = [[x_t, y_t], [x_t, y_b], [x_b, y_b], [x_b, y_t]]
    line_square = [Line([x_t, y_t], [x_t, y_b]),   # ^--------->*
                   Line([x_t, y_b], [x_b, y_b]),   # |          |
                   Line([x_b, y_b], [x_b, y_t]),   # |          |
                   Line([x_b, y_t], [x_t, y_t])]   # |<---------v
    
    crs = [[isCross(line_AB, line), line] for line in line_square if isCross(line_AB, line)]

    crossPoints = []
    if crs:
        crossPoints = [findIntersection(line_AB, crs[i][1]) for i in range(len(crs))]
        midPoint = Position((crossPoints[0][0]+crossPoints[1][0])/2, (crossPoints[0][1]+crossPoints[1][1])/2)
        return midPoint
    else:
        bl1 = [pointA[0] < x_b and pointB[0] < x_b, x_b]
        bl2 = [pointA[0] > x_t and pointB[0] > x_t, x_t]
        bl3 = [pointA[1] < y_b and pointB[1] < y_b, y_b]
        bl4 = [pointA[1] > y_t and pointB[1] > y_t, y_t]

        count = []
        for i in [bl1, bl2, bl3, bl4]:
            if i[0]:
                count.append(i[1])

        if len(count) != 1:  # 当时这里忘写了，导致在运行时返回None
            p = point_square[0]
            for point in point_square:
                if calculateDistance(point, pointA) + calculateDistance(point, pointB) < calculateDistance(p, pointA) + calculateDistance(p, pointB):
                    p = point
            return p
        else:
            axis = count[0]

            if axis == x_b or axis == x_t:
                x_A_, y_A_ = 2 * axis - pointA[0], pointA[1]
                mirrorLine = line_square[2] if axis == x_b else line_square[0]
                refLine = Line([x_A_, y_A_], [pointB[0], pointB[1]])
                if findIntersection(refLine, mirrorLine):
                    return findIntersection(refLine, mirrorLine)
                else:
                    if axis == x_b:
                        return [x_b, y_t] if (calculateDistance([x_b, y_t], pointB) + calculateDistance([x_b, y_t], pointA)) < (calculateDistance([x_b, y_b], pointB) + calculateDistance([x_b, y_b], pointA)) else [x_b, y_b]
                    else:
                        return [x_t, y_t] if (calculateDistance([x_t, y_t], pointB) + calculateDistance([x_t, y_t], pointA)) < (calculateDistance([x_t, y_b], pointB) + calculateDistance([x_t, y_b], pointA)) else [x_t, y_b]
            
            if axis == y_b or axis == y_t:
                x_A_, y_A_ = pointA[0], 2 * axis - pointA[1]
                mirrorLine = line_square[1] if axis == y_b else line_square[3]
                refLine = Line([x_A_, y_A_], [pointB[0], pointB[1]])
                if findIntersection(refLine, mirrorLine):
                    return findIntersection(refLine, mirrorLine)
                else:
                    if axis == y_b:
                        return [x_t, y_b] if (calculateDistance([x_t, y_b], pointB) + calculateDistance([x_t, y_b], pointA)) < (calculateDistance([x_b, y_b], pointB) + calculateDistance([x_b, y_b], pointA)) else [x_b, y_b]
                    else:
                        return [x_t, y_t] if (calculateDistance([x_t, y_t], pointB) + calculateDistance([x_t, y_t], pointA)) < (calculateDistance([x_b, y_t], pointB) + calculateDistance([x_b, y_t], pointA)) else [x_b, y_t]
                    

"""
state: bug fixing
"""
# find a way to destination while avoiding barriers
# return a list of points that the vehicle should go through
def barrierAvoidance(vehicle:Vehicle, endPos:Position, startPos:Position, extendedBarriers:list[ExtendedBarrier], client:VehicleClient):
    routePoints = []    #list(Position), holding the points that the vehicle should go through

    # 获取车辆起始位置和目标位置
    # 如果起始位置未指定，则获取当前车辆位置
    if startPos is None:
        startPos = getPureCurrentVehiclelPos(vehicle.ID, client)
    else:
        startPos = (startPos.x, startPos.y)
    startNode = (startPos[0], startPos[1])  # 起始节点(元组格式)
    endNode = (endPos.x, endPos.y)          # 目标节点(元组格式)

    # # 创建扩展障碍物（比原始障碍物大5米）
    # extendedBarriers = [ExtendedBarrier(bar) for bar in barriers]

    # 收集所有可能的路径节点：起始点、目标点、所有扩展障碍物的顶点
    allNodes = set()
    allNodes.add(startNode)  # 添加起始点
    allNodes.add(endNode)    # 添加目标点
    
    # 添加所有扩展障碍物的顶点作为潜在路径节点
    for barrier in extendedBarriers:
        if hasattr(barrier.shape, 'vertices') and barrier.shape.vertices:
            for vertex in barrier.shape.vertices:
                if vertex not in Conv(barrier.shape.vertices):  # 排除凹顶点
                    allNodes.add(tuple(vertex)) # 转换为元组便于比较

    # A*算法初始化
    openSet = {startNode}    # 待探索的节点集合
    closedSet = set()        # 已探索的节点集合
    
    # gScore: 从起点到当前节点的实际代价
    gScore = {startNode: 0}
    
    # fScore: 从起点经过当前节点到终点的估计总代价 (f = g + h)
    fScore = {startNode: calculateDistance(startNode, endNode)}
    
    # cameFrom: 记录每个节点的前驱节点，用于重建路径
    cameFrom = {}
    
    # A*主循环
    while openSet:
        # 从openSet中选择fScore最小的节点作为当前节点
        current = min(openSet, key=lambda node: fScore.get(node, float('inf')))
        
        # 如果当前节点就是目标节点，路径找到
        if current == endNode:
            # 重建路径并转换为Position对象列表
            pathNodes = reconstructPath(cameFrom, current)
            routePoints = [listToPosition(node) for node in pathNodes]
            return routePoints
        
        # 将当前节点从openSet移到closedSet
        openSet.remove(current)
        closedSet.add(current)
        
        # 探索当前节点的所有邻居节点（所有其他路径节点）
        for neighbor in allNodes:
            if neighbor == current or neighbor in closedSet:
                continue  # 跳过自身节点和已探索节点
            
            # 检查从当前节点到邻居节点的直线路径是否与任何障碍物碰撞
            if isPathColliding(current, neighbor, extendedBarriers):
                continue  # 如果路径碰撞，跳过该邻居
            
            # 计算从起点经过当前节点到邻居节点的实际代价
            tentative_gScore = gScore[current] + calculateDistance(current, neighbor)
            
            # 如果邻居节点不在openSet中，或者找到更优路径
            if neighbor not in openSet:
                openSet.add(neighbor)  # 添加到待探索集合
            elif tentative_gScore >= gScore.get(neighbor, float('inf')):
                continue  # 如果不是更优路径，跳过
            
            # 更新路径信息：记录前驱节点和代价
            cameFrom[neighbor] = current
            gScore[neighbor] = tentative_gScore
            fScore[neighbor] = tentative_gScore + calculateDistance(neighbor, endNode)
    
    # 如果找不到安全路径，返回直接路径（可能会碰撞）
    return [endPos]


def reconstructPath(cameFrom, current):
    """
    根据cameFrom字典重建完整路径
    从目标节点反向追踪到起始节点
    """
    path = [current]
    while current in cameFrom:
        current = cameFrom[current]
        path.append(current)
    path.reverse()  # 反转路径，从起点到终点
    return path[1:]


def isPathColliding(start, end, extendedBarriers):
    """
    检查从start到end的直线路径是否与任何扩展障碍物碰撞
    返回True如果碰撞，False如果安全
    """
    # 创建线段对象用于碰撞检测
    pathLine = Line([start[0], start[1]], [end[0], end[1]])
    
    # 检查与所有扩展障碍物的碰撞
    for barrier in extendedBarriers:
        if barrierCollision(pathLine, barrier):
            return True  # 路径与障碍物碰撞
    
    return False  # 路径安全


def barrierCollision(line, barrier:ExtendedBarrier):
    """
    检查线段是否与单个障碍物碰撞
    """
    if barrier.shape.nZoneType == 0 or barrier.shape.nZoneType == 3:  # 多边形或矩形
        # 检查线段是否与障碍物的任何边相交
        for side in barrier.sides:
            if isCross(line, side):
                return True
        
        middleX = (line.startPos[0] + line.endPos[0]) / 2
        middleY = (line.startPos[1] + line.endPos[1]) / 2
        if isInsideArea([middleX, middleY], barrier):
            return True
            
    elif barrier.shape.nZoneType == 1:  # 圆形障碍物
        # 检查线段与圆的碰撞
        if lineCircleCollision(line, barrier.shape.center, barrier.shape.radius):
           return True
    
    return False


"""
state: bug fixing
"""
def lineCircleCollision(line, center, radius):
    """
    检查线段是否与圆碰撞
    算法：计算线段上离圆心最近的点，检查距离是否小于半径
    """
    # 线段向量
    line_vec = [line.endPos[0] - line.startPos[0], line.endPos[1] - line.startPos[1]]
    line_length = math.sqrt(line_vec[0]**2 + line_vec[1]**2)
    
    if line_length == 0:
        # 线段退化为点，直接检查点到圆心距离
        return calculateDistance(line.startPos, center) <= radius
    
    # 线段方向单位向量
    line_dir = [line_vec[0] / line_length, line_vec[1] / line_length]
    
    # 从线段起点到圆心的向量
    to_center = [center[0] - line.startPos[0], center[1] - line.startPos[1]]
    
    # 投影长度：to_center在line_dir上的投影
    projection = to_center[0] * line_dir[0] + to_center[1] * line_dir[1]
    
    # 钳制投影到线段范围内 [0, line_length]
    projection = max(0, min(line_length, projection))
    
    # 计算线段上离圆心最近的点
    closest_point = [
        line.startPos[0] + projection * line_dir[0],
        line.startPos[1] + projection * line_dir[1]
    ]
    
    # 检查最近点到圆心的距离
    return calculateDistance(closest_point, center) <= radius


"""
state: developing
"""
def judgePriority(vehicle1:Vehicle, vehicle2:Vehicle):
    def setPriority(vehicle:Vehicle):
        vehicle.refreshPriority()
        if len(vehicle.content) != 0:
            if vehicle.type == "large":
                vehicle.addPriority(100)
            elif vehicle.type == "medium":
                vehicle.addPriority(90)
            elif vehicle.type == "small":
                vehicle.addPriority(80)

            if vehicle.content[0].weight == "overload":
                vehicle.addPriority(4)
            elif vehicle.content[0].weight == "heavy":
                vehicle.addPriority(3)
            elif vehicle.content[0].weight == "medium":
                vehicle.addPriority(2)
            elif vehicle.content[0].weight == "light":
                vehicle.addPriority(1)
        
        else:
            if vehicle.type == "large":
                vehicle.addPriority(50)
            elif vehicle.type == "medium":
                vehicle.addPriority(40)
            elif vehicle.type == "small":
                vehicle.addPriority(30)
    
    setPriority(vehicle1)
    setPriority(vehicle2)

    return vehicle1.ID if vehicle1.priority < vehicle2.priority else vehicle2.ID


"""
state: developing
"""
def avoidVehicleCollision(thisVehicle:Vehicle, otherVehicle:Vehicle, this_curLine:Line, other_curLine:Line, extendedBarriers:list[ExtendedBarrier], client:VehicleClient):
    """
    避免两车碰撞
    Parameter:
        this_curLine: list[startPos, endPos] 当前路径线段的起点和终点
        other_curLine: list[startPos, endPos]
    """
    thisVel = calculate_real_speed(thisVehicle, client)
    otherVel = calculate_real_speed(otherVehicle, client)
    thisVelDir = getSpeedDirection(thisVehicle, client)
    otherVelDir = getSpeedDirection(otherVehicle, client)
    dVehiclePhi = otherVelDir - thisVelDir
    thisPos = getPureCurrentVehiclelPos(thisVehicle.ID, client)
    otherPos = getPureCurrentVehiclelPos(otherVehicle.ID, client)
    currentVehicleDist = client.get_distance_by_lon_lat(Position(thisPos[0], thisPos[1]), Position(otherPos[0], otherPos[1]))

    k2 = math.tan(otherVelDir)
    k1 = math.tan(thisVelDir)
    x1, y1 = thisPos[0], thisPos[1]
    x2, y2 = otherPos[0], otherPos[1]
    def f1(x):
        return k1 * (x - x1) + y1
    def f2(x):
        return k2 * (x - x2) + y2
    def degreeToRadian(degree) -> float:
        return degree * math.pi / 180
    def getFootPoint(point:list[float, float], line:list[float, float]) -> list[float, float]:
        """
        计算点(x, y)到直线y = k*x + b的垂足(x_0, y_0)

        Parameter:
            point: list[x, y]
            line: list[k, b]
        Return: 
            list[x_0, y_0]
        """
        x, y = point[0], point[1]
        k, b = line[0], line[1]
        x_0 = (x - k*b + k*y) / (1 + k**2)
        y_0 = k * x_0 + b
        return [x_0, y_0]
    def disPointToLine(point:list[float, float], line:list[float, float]) -> float:
        """
        计算点到直线的距离 d = |A * x_0 + B * y_0 + C| / sqrt(A^2 + B^2)

        Parameter:
            point: list[x, y]
            line: list[a, b, c]
        Return: 
            float
        """
        a, b, c = line[0], line[1], line[2]
        x_0, y_0 = point[0], point[1]
        return abs(a*x_0 + b*y_0 + c) / math.sqrt(a**2 + b**2)

    # (如果两辆车的当前路径线段相同，且this的速度小于other) 或者 路径相同相向而行
    if (this_curLine == other_curLine and thisVel < otherVel) or (this_curLine.endPos == other_curLine.startPos and this_curLine.startPos == other_curLine.endPos):
        phi = degreeToRadian(otherVelDir + 90)
        safePoint = meterToLatLon(12.0, thisPos, math.sin(phi), math.cos(phi))
        if isInsideArea(safePoint, extendedBarriers):
            phi = degreeToRadian(otherVelDir - 90)
            safePoint = meterToLatLon(12.0, thisPos, math.sin(phi), math.cos(phi))
        maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, safePoint, thisVehicle.velocity, False)
        while True:
            try:
                status = client.get_command_status(maneuver_id)
                if status.get("status") == "EXECUTE_SUCCESS":
                    break
            except Exception:
                continue
        t = COLLISION_THRESHOLD * 2 / otherVel
        time.sleep(t)

        maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, thisPos, thisVehicle.velocity, False)
        while True:
            try:
                status = client.get_command_status(maneuver_id)
                if status.get("status") == "EXECUTE_SUCCESS":
                    break
            except Exception:
                continue
        
        maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, this_curLine.endPos, thisVehicle.velocity, False)
        while True:
            try:
                status = client.get_command_status(maneuver_id)
                if status.get("status") == "EXECUTE_SUCCESS":
                    break
            except Exception:
                continue

        return True
    
    # 如果两车的路径相交
    if isCross(this_curLine, other_curLine):
        # 两条直线的ABC参数
        a_1, b_1, c_1 = k1, -1, y1-k1*x1
        a_2, b_2, c_2 = k2, -1, y2-k2*x2
        d = disPointToLine([x1, y1], [a_2, b_2, c_2]) 
        if d >= 12.0:
            maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, [listToPosition(getPureCurrentVehiclelPos(thisVehicle.ID, client))], 0, False)
            t = 0
            while True:
                t += 1
                time.sleep(1)
                if disPointToLine([x2, y2], [a_1, b_1, c_1]) > COLLISION_THRESHOLD or t >= calculateDistance(other_curLine.startPos, other_curLine.endPos) / otherVel:
                    break

            maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, this_curLine.endPos, thisVehicle.velocity, False)
            while True:
                try:
                    status = client.get_command_status(maneuver_id)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue
            return True
        
        else:
            foot = getFootPoint([x1, y1], [a_2, b_2, c_2])
            if x1 != foot[0]:
                k = (foot[1] - y1) / (foot[0] - x1)
                x_safe1 = (12 * math.sqrt(a_2**2 + b_2**2) - (c_2 + b_2*(y1-k*x1))) / (a_2+b_2*k)
                x_safe2 = (-12 * math.sqrt(a_2**2 + b_2**2) - (c_2 + b_2*(y1-k*x1))) / (a_2+b_2*k)
                y_safe1 = k * x_safe1 + y1 - k*x1
                y_safe2 = k * x_safe2 + y1 - k*x1
            else:
                x_safe = x1
                y_safe1 = a_2 * x_safe + c_2 + 12
                y_safe2 = a_2 * x_safe + c_2 - 12
            if calculateDistance(thisPos, [x_safe, y_safe1]) < calculateDistance(thisPos, [x_safe, y_safe2]):
                safePoint = [x_safe, y_safe1]
            else:
                safePoint = [x_safe, y_safe2]

            client.set_tactical_maneuver(thisVehicle.ID, [listToPosition(safePoint)], thisVehicle.velocity, False)
            while True:
                t += 1
                time.sleep(1)
                if disPointToLine([x2, y2], [a_1, b_1, c_1]) > COLLISION_THRESHOLD or t >= calculateDistance(other_curLine.startPos, other_curLine.endPos) / otherVel:
                    break

            maneuver_id = client.set_tactical_maneuver(thisVehicle.ID, this_curLine.endPos, thisVehicle.velocity, False)
            while True:
                try:
                    status = client.get_command_status(maneuver_id)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue
            return True
    return False


"""
  -------------multiple vehicle parts----------------
"""


class CollabTask(Task):
    """
    多车协同任务类：同一取货区、同等重量且目的地相同的货物归为一类
    """
    def __init__(self, pick_area_id, delivery_area_id, goods_type, goods_weight, goods_list):
        # 继承Task的基本属性
        super().__init__(goods_list[0])
        self.pick_area_id = pick_area_id
        self.delivery_area_id = delivery_area_id
        self.goods_type = goods_type
        self.goods_weight = goods_weight
        self.goods_list = goods_list  # 该类任务下所有货物（Package对象）
        self.status = "pending"
        self.assigned_vehicles = []
        self.start_time = None
        self.end_time = None
        self.effective_follow_time = 0
        self.sample_count = 0

class CollabVehicle(Vehicle):
    """拓展Vehicle类，增加碰撞记录与协同状态同步"""
    def __init__(self, ID, capacity, velcoity, vType, startPos: Position):
        super().__init__(ID, capacity, velcoity, vType, startPos)
        self.collision_count = 0  # 碰撞次数（用于安全性评分）
        self.cur_task_id = None    # 当前绑定任务ID
        self.is_returning = False # 是否在返回起始区

    def update_collision(self, collision_data: dict):
        """更新碰撞次数（从仿真平台获取）"""
        self.collision_count = collision_data.get(self.ID, 0)

    def is_collab_valid(self, other_vehicle, client:VehicleClient) -> bool:
        """判断与另一车辆的协同状态是否有效"""
        # 1. 距离检查：≤协同紧密距离
        dist = client.get_distance_by_lon_lat(
            Position(getPureCurrentVehiclelPos(self, client)[0], getPureCurrentVehiclelPos(self, client)[1]),
            Position(getPureCurrentVehiclelPos(other_vehicle, client)[0], getPureCurrentVehiclelPos(other_vehicle, client)[1])
        )
        # 2. 速度检查：速度差≤阈值
        speed_diff = np.isclose(calculate_real_speed(self), calculate_real_speed(other_vehicle))
        return dist <= COLLAB_DIST and speed_diff


# 将在vehicleRun中启动监视器线程，并在每辆车线程内处理预警
def collision_monitor_thread(vehicles, client, warning_queue, stop_event):
    """
    全局碰撞监视器线程
    持续监控所有车辆间的距离，若发现即将碰撞，则将相关车辆ID对放入warning_queue
    stop_event用于退出线程
    """
    while not stop_event.is_set():
        positions = {v.ID: getPureCurrentVehiclelPos(v.ID, client) for v in vehicles}
        vehicle_ids = list(positions.keys())
        for i in range(len(vehicle_ids)):
            for j in range(i+1, len(vehicle_ids)):
                id1, id2 = vehicle_ids[i], vehicle_ids[j]
                pos1, pos2 = positions[id1], positions[id2]
                # 计算两车距离
                dist = client.get_distance_by_lon_lat(Position(pos1[0], pos1[1]), Position(pos2[0], pos2[1]))
                if dist < COLLISION_THRESHOLD:
                    # 发现即将碰撞，放入预警队列
                    warning_queue.put((id1, id2))
        time.sleep(COLLSION_TEST_TIME)  # 监控频率，可根据实际调整


def generate_all_routes(vehicles, pick_areas, delivery_areas, barriers, client):
    """
    多线程预生成所有车辆的完整路径，返回routeList字典
    routeList = {vehicleID: [route1, route2, ...], ...}
    """
    routeList = {}

    def gen_vehicle_routes(vehicle):
        routes = []
        # 起点到所有取货区
        for pick_area in pick_areas:
            centerPos = Position(pick_area.shape.center[0], pick_area.shape.center[1])
            route = barrierAvoidance(vehicle, centerPos, vehicle.startPos, barriers, client)
            routes.append(route)
        # 取货区到所有送货区
        for delivery_area in delivery_areas:
            startPos = Position(pick_area.shape.center[0], pick_area.shape.center[1])
            centerPos = Position(delivery_area.shape.center[0], delivery_area.shape.center[1])
            route = barrierAvoidance(vehicle, centerPos, startPos, barriers, client)
            routes.append(route)
        # 送货区回起点
        centerPos = Position(delivery_area.shape.center[0], delivery_area.shape.center[1])
        route = barrierAvoidance(vehicle, vehicle.startPos, centerPos, barriers, client)
        routes.append(route)
        return (vehicle.ID, routes)

    with concurrent.futures.ThreadPoolExecutor() as executor:
        future_to_vehicle = {executor.submit(gen_vehicle_routes, v): v for v in vehicles}
        for future in concurrent.futures.as_completed(future_to_vehicle):
            vid, routes = future.result()
            routeList[vid] = routes
    return routeList


def classify_collab_tasks(pick_areas):
    """
    任务分类辅助：同一取货区、同等重量且目的地相同的货物归为一类，作为任务分配函数的基础
    返回：[(pick_area_id, delivery_area_id, weight): [Package, ...], ...]
    """
    task_dict = {}
    for pick_area in pick_areas:
        for pkg in pick_area.content:
            key = (pick_area.ID, pkg.destinationID, pkg.weight)
            if key not in task_dict:
                task_dict[key] = []
            task_dict[key].append(pkg)
    return task_dict


"""
state: developing
"""
def assign_tasks(vehicles, pick_areas, delivery_areas):
    """
    任务分配：同一取货区、同等重量且目的地相同的货物归为一类，循环调动车辆，一个一个送
    优先级撤步：超限>普通
    支持协同与超限货物
    """
    tasks = []
    task_dict = classify_collab_tasks(pick_areas)
    vehicle_idx = 0
    vehicle_count = len(vehicles)
    for (pick_area_id, delivery_area_id, weight), pkg_list in task_dict.items():
        # 判断是否需要协同（超限）
        max_cap = max(v.capacity for v in vehicles)
        goods_type = pkg_list[0].type
        if weight > max_cap or goods_type == "overload":
            # 协同任务：分配两辆载重最大的车
            sorted_vehicles = sorted([v for v in vehicles if v.cur_task_id is None], key=lambda v: v.capacity, reverse=True)
            assigned = sorted_vehicles[:2]
            task = CollabTask(pick_area_id, delivery_area_id, goods_type, weight, pkg_list)
            task.assigned_vehicles = [v.ID for v in assigned]
            for v in assigned:
                v.cur_task_id = f"collab_{pick_area_id}_{delivery_area_id}_{weight}"
                v.add_collaborator([vid for vid in task.assigned_vehicles if vid != v.ID][0])
                v.is_collaborating = True
            tasks.append(task)
        else:
            # 单车任务，循环调度车辆
            for pkg in pkg_list:
                available_vehicles = [v for v in vehicles if v.cur_task_id is None]
                if not available_vehicles:
                    # 若无空闲车辆，循环分配
                    v = vehicles[vehicle_idx % vehicle_count]
                else:
                    v = available_vehicles[0]
                task = CollabTask(pick_area_id, delivery_area_id, goods_type, weight, [pkg])
                task.assigned_vehicles = [v.ID]
                v.cur_task_id = f"single_{pkg.ID}"
                tasks.append(task)
                vehicle_idx += 1
    # 放入全局队列
    for t in tasks:
        task_queue.put(t)
    return tasks


"""
state: developing
"""
def execute_collab_task(vehicle, task, vehicles, barriers, pick_areas, delivery_areas, client, routeList=None):
    """
    执行协同任务/单车任务
    1. 多线程锁保证协同安全
    2. 通信同步
    3. 利用预生成路径
    修正版：协同任务采用Barrier同步，保证assigned_vehicles全部到达后再取货/送货
    """
    # 步骤1：获取任务关联的取货/送货区
    with area_content_lock:
        pick_area = next((area for area in pick_areas if area.ID == task.pick_area_id), None)
        delivery_area = next((area for area in delivery_areas if area.ID == task.delivery_area_id), None)
    
    if not pick_area or not delivery_area:
        print(f"车辆{vehicle.ID}无法找到任务{task.task_id}对应的区域")
        return
    
     # 新增：为协同任务分配Barrier（只分配一次）
    barrier = None
    if len(task.assigned_vehicles) > 1:
        with task_lock:
            if not hasattr(task, "task_id"):
                # 兼容老任务类
                task.task_id = f"{task.pick_area_id}_{task.delivery_area_id}_{task.goods_weight}"
            if task.task_id not in collab_barrier_dict:
                collab_barrier_dict[task.task_id] = threading.Barrier(len(task.assigned_vehicles))
        barrier = collab_barrier_dict[task.task_id]

    # 协同车辆
    collab_vehicles = [v for v in vehicles if v.ID in task.assigned_vehicles and v.ID != vehicle.ID]

    
    # 步骤2：前往取货区（复用algorithm.py的路径规划）
    print(f"车辆{vehicle.ID}前往取货区{pick_area.ID}（任务{task.task_id}）")
    # target_area = pick_area
    
    while True:
        with vehicle_status_lock:
            current_pos = getPureCurrentVehiclelPos(vehicle.ID, client)
            if isInsideArea(current_pos, pick_area):
                break

        # 优先使用预生成路径
        if routeList and vehicle.ID in routeList and len(routeList[vehicle.ID]) > 0:
            route1 = routeList[vehicle.ID][0]
        else:
            startPos = getPureCurrentVehiclelPos(vehicle.ID, client)
            centerPos = Position(pick_area.shape.center[0], pick_area.shape.center[1])
            route1 = barrierAvoidance(vehicle, pick_area.shape.center, startPos, barriers, client)
        # 再计算离开pick_area去delivery_area的路径route2
        if routeList and vehicle.ID in routeList and len(routeList[vehicle.ID]) > 1:
            route2 = routeList[vehicle.ID][1]
        else:
            startPos = getPureCurrentVehiclelPos(vehicle.ID, client)
            centerPos = Position(delivery_area.shape.center[0], delivery_area.shape.center[1])
            route2 = barrierAvoidance(vehicle, centerPos, startPos, barriers, client)
        # 计算目标点
        target_pos = setDestinationPos(vehicle, route1, route2, pick_area)
        route_points  = route1[:-1]
        tempEndRoute = barrierAvoidance(vehicle, target_pos, route1[-2], barriers, client)
        route_points.extend(tempEndRoute)

        for point in route_points:
            maneuver_id = client.set_tactical_maneuver(
                vehicle_id=vehicle.ID,
                position=[point],
                speed=vehicle.velocity,
                ispatrol=False
            )
            while True:
                try:
                    status = client.get_command_status(maneuver_id)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue
        
    # 协同等待（屏障同步）
    if barrier:
        print(f"车辆{vehicle.ID}等待协同车辆到达取货区（任务{task.task_id}）")
        try:
            barrier.wait(timeout=60) 
            # 所有assigned_vehicles线程到达后才继续
            # 最多等待60秒，防止死锁
        except threading.BrokenBarrierError:
            print(f"协同任务{task.task_id} Barrier异常，车辆{vehicle.ID}退出")
            return

    # 步骤3：取货
    with task_lock, area_content_lock:
        for pkg in task.goods_list:
            pickUpGoods(vehicle, pick_area, pkg.ID, client)
        print(f"车辆{vehicle.ID}在取货区{pick_area.ID}取货")
    with task_lock:
        if task.status == "pending":
            task.status = "transporting"
            task.start_time = client.get_sim_time()

    # 步骤4：前往送货区
    print(f"车辆{vehicle.ID}前往送货区{delivery_area.ID}（任务{task.task_id}）")
    # target_area = delivery_area

    while True:
        with vehicle_status_lock:
            current_pos = getPureCurrentVehiclelPos(vehicle.ID, client)
            if isInsideArea(current_pos, delivery_area):
                break
        if routeList and vehicle.ID in routeList and len(routeList[vehicle.ID]) > 1:
            route2 = routeList[vehicle.ID][1]
        else:
            startPos = getPureCurrentVehiclelPos(vehicle.ID, client)
            centerPos = Position(delivery_area.shape.center[0], delivery_area.shape.center[1])
            route2 = barrierAvoidance(vehicle, centerPos, startPos, barriers, client)
        
        """
        此处需要优化收货点位置，暂定使用收货区中心
        """
        
        vel = min(v.velocity for v in collab_vehicles)

        for point in route2:
            maneuver_id = client.set_tactical_maneuver(
                vehicle_id=vehicle.ID,
                position=[point],
                speed=vel,
                ispatrol=False
            )
            while True:
                try:
                    status = client.get_command_status(maneuver_id)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue

    # 协同等待（屏障同步），同取货区
    if barrier:
        print(f"车辆{vehicle.ID}等待协同车辆到达送货区（任务{task.task_id}）")
        try:
            barrier.wait(timeout=60)
        except threading.BrokenBarrierError:
            print(f"协同任务{task.task_id} Barrier异常，车辆{vehicle.ID}退出")
            return

    # 送货
    with task_lock, area_content_lock:
        for pkg in task.goods_list:
            sendInGoods(vehicle, delivery_area, client)
    with task_lock:
        if task.status != "completed":
            task.status = "completed"
            task.end_time = client.get_sim_time()


    # 步骤6：返回起始区
    while True:
        with vehicle_status_lock:
            if isBackToStart(vehicle, client):
                break
        if routeList and vehicle.ID in routeList and len(routeList[vehicle.ID]) > 2:
            route3 = routeList[vehicle.ID][2]
        else:
            startPos = getPureCurrentVehiclelPos(vehicle.ID, client)
            route3 = barrierAvoidance(vehicle, vehicle.startPos, startPos, barriers, client)
        
        for point in route3:
            maneuver_id = client.set_tactical_maneuver(
                vehicle_id=vehicle.ID,
                position=[point],
                speed=vehicle.velocity,
                ispatrol=False
            )
            while True:
                try:
                    status = client.get_command_status(maneuver_id)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue

    with task_lock, vehicle_status_lock:
        vehicle.cur_task_id = None
        vehicle.is_collaborating = False
        vehicle.collaborator_ids = []

    print(f"车辆{vehicle.ID}完成任务{task.task_id}，已返回起始区")
    
    # 协同任务完成后清理Barrier，防止内存泄漏
    if barrier and barrier.n_waiting == 0:
        with task_lock:
            collab_barrier_dict.pop(task.task_id, None)


"""
state: developing
"""
# def vehicleRun(vehicles: list[CollabVehicle], pick_areas: list[Pick], delivery_areas: list[Delivery], barriers: list[Barrier], client):
#     # 步骤0：多线程预生成所有路径
#     print("正在预生成所有车辆路径...")
#     routeList = generate_all_routes(vehicles, pick_areas, delivery_areas, barriers, client)
#     print("路径预生成完成。")

#     # 步骤1：初始化任务分配
#     tasks = assign_tasks(vehicles, pick_areas, delivery_areas)
#     print(f"初始化完成{len(tasks)}个任务分配")

#     # 步骤2：创建并启动线程
#     # 启动全局碰撞监视器线程
#     stop_event = threading.Event()
#     monitor_thread = threading.Thread(target=collision_monitor_thread, args=(vehicles, client, collision_warning_queue, stop_event))
#     monitor_thread.start()
#     #启动多车并行线程
#     threads = []
    
#     def vehicle_thread(vehicle, routeList):
#         while True:
#             # 检查是否有针对本车的碰撞预警
#             try:
#                 while True:
#                     id1, id2 = collision_warning_queue.get_nowait()
#                     if vehicle.ID in (id1, id2):
#                         vehicle1 = [v for v in vehicles if v.ID == id1][0]
#                         vehicle2 = [v for v in vehicles if v.ID == id2][0]
#                         if judgePriority(vehicle1, vehicle2) == vehicle.ID:
#                             avoidVehicleCollision(vehicle, vehicle2, client)
#                             print(f"车辆{vehicle.ID}收到与车辆{(id2 if vehicle.ID == id1 else id1)}的碰撞预警，触发避障")
#             except queue.Empty:
#                 pass

#             try:
#                 # 线程安全地从队列取任务
#                 task = task_queue.get_nowait()
#             except queue.Empty:
#                 break

#             # 判断该任务是否分配给本车
#             if vehicle.ID in task.assigned_vehicles:
#                 execute_collab_task(vehicle, task, vehicles, barriers, pick_areas, delivery_areas, client, routeList)
#                 # 任务完成后，车辆状态重置，允许分配下一个任务
#                 with task_lock, vehicle_status_lock:
#                     vehicle.cur_task_id = None
#                     vehicle.is_collaborating = False
#                     vehicle.collaborator_ids = []
#             else:
#                 # 非本车任务，放回队列尾部
#                 task_queue.put(task)
#                 time.sleep(0.05)  # 防止死循环CPU占用

#     for vehicle in vehicles:
#         thread = threading.Thread(target=vehicle_thread, args=(vehicle, routeList))
#         threads.append(thread)
#         thread.start()

#     for thread in threads:
#         thread.join()

#     # 关闭监视器线程
#     stop_event.set()
#     monitor_thread.join()  
                          
#     # 步骤4：输出任务完成统计
#     with task_lock:
#         completed_tasks = [t for t in tasks if t.status == "completed"]
#         print(f"\n任务完成统计：共{len(tasks)}个任务，完成{len(completed_tasks)}个")

#     for task in tasks:
#         print(f"任务{task.task_id}（{task.goods_type}）：状态={task.status}，运输时长={task.end_time - task.start_time if task.end_time else '未完成'}秒")
#         if task.goods_type == "overload":
#             collab_rate = task.effective_follow_time / task.sample_count if task.sample_count > 0 else 0
#             print(f"  协同性：有效跟随时长={task.effective_follow_time}秒，协同率={collab_rate:.2%}")

#     # 步骤5：输出安全性统计
#     with vehicle_status_lock:
#         total_collision = sum(getattr(v, "collision_count", 0) for v in vehicles)
#         print(f"\n安全性统计：总碰撞次数={total_collision}次")


def optimize_delivery_order(vehicle: Vehicle, tasks: list[Task], delivery_areas: list[Delivery]) -> list[Task]:
    """优化送货顺序，使用贪心算法最小化总路径长度"""
    if not tasks:
        return []
    
    # 构建送货区ID到坐标的映射
    delivery_pos_map = {
        area.ID: area.shape.center 
        for area in delivery_areas
    }
    
    # 初始位置为车辆当前位置
    current_pos = (vehicle.startPos.x, vehicle.startPos.y)
    remaining_tasks = tasks.copy()
    optimized_tasks = []
    
    while remaining_tasks:
        # 找到距离当前位置最近的送货点
        min_dist = float('inf')
        selected_task = None
        
        for task in remaining_tasks:
            dest_pos = delivery_pos_map.get(task.delivery_area_id)
            if not dest_pos:
                continue
                
            dist = calculateDistance(current_pos, dest_pos)
            if dist < min_dist:
                min_dist = dist
                selected_task = task
        
        if selected_task:
            optimized_tasks.append(selected_task)
            remaining_tasks.remove(selected_task)
            current_pos = delivery_pos_map[selected_task.delivery_area_id]
    
    return optimized_tasks


def vehicle_multi_delivery(vehicle: Vehicle, pick_areas: list[Pick], delivery_areas: list[Delivery], 
                          barriers: list[ExtendedBarrier], client: VehicleClient):
    """车辆多货物配送主逻辑"""
    # 1. 获取可分配的货物（筛选符合车辆类型且未分配的货物）
    available_tasks = []
    with area_content_lock:
        for pick_area in pick_areas:
            # 筛选车辆可承载的货物（重量匹配）
            for pkg in pick_area.content:
                if vehicle.capacity >= pkg.weight and pkg.status == 0:
                    available_tasks.append(Task(pkg))
    
    # 2. 根据车辆容量筛选最大可承载货物数量

    max_load = vehicle.capacity
    selected_tasks = []
    total_weight = 0
    used_pkg_ids = set()
    for task in available_tasks:
        pkg = task.package
        if pkg is None:
            continue
        # 避免重复装载同一包裹
        if pkg.ID in used_pkg_ids:
            continue
        # 判断累计重量是否超载
        if total_weight + pkg.weight <= max_load:
            selected_tasks.append(task)
            total_weight += pkg.weight
            used_pkg_ids.add(pkg.ID)
        # 已达最大承载量则停止
        if total_weight >= max_load:
            break
    if not selected_tasks:
        return
    
    # 3. 优化送货顺序
    optimized_tasks = optimize_delivery_order(vehicle, selected_tasks, delivery_areas)
    
    # 4. 集中取货（前往取货区装载所有货物）
    pick_area = next(pa for pa in pick_areas if pa.ID == optimized_tasks[0].pick_area_id)
    pick_pos = pick_area.shape.center
    
    # 规划到取货区的路径
    currentVehiclePos = getPureCurrentVehiclelPos(vehicle.ID, client)
    temp_pick_route = barrierAvoidance(vehicle, listToPosition(pick_pos), listToPosition(currentVehiclePos), barriers, client)
    nextDestPoint = [da.shape.center for da in delivery_areas if da.ID == optimized_tasks[0].delivery_area_id][0]
    nextRoute = barrierAvoidance(vehicle, listToPosition(nextDestPoint), listToPosition(pick_pos), barriers, client)
    smallEnd = nextRoute[0]
    smallStart = temp_pick_route[-2] if len(temp_pick_route) > 1 else listToPosition(currentVehiclePos)
    pickPos = getRefinementPoint(pick_area, smallStart, smallEnd)
    pick_route = barrierAvoidance(vehicle, listToPosition(pickPos), listToPosition(currentVehiclePos), barriers, client)
    realNextRoute = barrierAvoidance(vehicle, listToPosition(nextDestPoint), listToPosition(pickPos), barriers, client)

    for waypoint in pick_route:
        stepRun = client.set_tactical_maneuver(vehicle.ID, [waypoint], vehicle.velocity, False)
        while True:
            try:
                status = client.get_command_status(stepRun)
                if status.get("status") == "EXECUTE_SUCCESS":
                    break
            except Exception:
                continue
    
    # 装载货物
    with area_content_lock, vehicle_status_lock:
        for task in optimized_tasks:
            pkg = next(p for p in pick_area.content if p.ID == task.task_id)
            pickUpGoods(vehicle, pick_area, pkg, client)
            task.status = "assigned"
    
    print('finish pick up')

    taskRouteList = []
    taskRouteList.append(realNextRoute)

    # 5. 依次配送货物
    for i in len(optimized_tasks):
        task = optimized_tasks[i]
        # 获取送货区位置
        delivery_area = next(da for da in delivery_areas if da.ID == task.delivery_area_id)
        delivery_pos = delivery_area.shape.center
        
        # 规划到送货区的路径
        currentVehiclePos = getPureCurrentVehiclelPos(vehicle.ID, client)
        temp_delivery_route = taskRouteList[i]
        nextDestPoint = listToPosition([da.shape.center for da in delivery_areas if da.ID == optimized_tasks[i+1].delivery_area_id][0]) if i < len(optimized_tasks)-1 else vehicle.startPos
        nextRoute = barrierAvoidance(vehicle, nextDestPoint, listToPosition(delivery_pos), barriers, client)
        smallEnd = nextRoute[0]
        smallStart = temp_delivery_route[-2] if len(temp_delivery_route) > 1 else listToPosition(currentVehiclePos)
        targetPos = getRefinementPoint(delivery_area, smallStart, smallEnd)
        delivery_route = barrierAvoidance(vehicle, listToPosition(targetPos), listToPosition(currentVehiclePos), barriers, client)
        realNextRoute = barrierAvoidance(vehicle, nextDestPoint, listToPosition(targetPos), barriers, client)
        taskRouteList.append(realNextRoute)

        for waypoint in delivery_route:
            stepRun = client.set_tactical_maneuver(vehicle.ID, [waypoint], vehicle.velocity, False)
            while True:
                try:
                    status = client.get_command_status(stepRun)
                    if status.get("status") == "EXECUTE_SUCCESS":
                        break
                except Exception:
                    continue
        
        # 卸货
        with area_content_lock, vehicle_status_lock:
            pkg = next((p for p in vehicle.content if p.ID == task.task_id), None)
            # vehicle.removeContent(pkg.ID)
            sendInGoods(vehicle, delivery_area, pkg, client)
            #pkg.status = 2  # 标记为已送达
            task.status = "completed"
            #task.end_time = time.time()
    
    # 6. 返回起点
    home_route = barrierAvoidance(vehicle, vehicle.startPos, None, barriers, client)
    for waypoint in home_route:
        stepRun = client.set_tactical_maneuver(vehicle.ID, [waypoint], vehicle.velocity, False)
        while True:
            try:
                status = client.get_command_status(stepRun)
                if status.get("status") == "EXECUTE_SUCCESS":
                    break
            except Exception:
                continue


def vehicleRun(vehicles: list[Vehicle], pickAreas: list[Pick], deliveryAreas: list[Delivery], 
              barriers: list[ExtendedBarrier], client: VehicleClient):
    """主算法入口，为每辆车分配多货物配送任务"""
    with concurrent.futures.ThreadPoolExecutor(max_workers=len(vehicles)) as executor:
        # 为每辆车启动多货物配送线程
        futures = [
            executor.submit(vehicle_multi_delivery, vehicle, pickAreas, deliveryAreas, barriers, client)
            for vehicle in vehicles
        ]
        
        # 等待所有任务完成
        for future in concurrent.futures.as_completed(futures):
            # try:
            #     future.result()
            # except Exception as e:
            #     print(f"车辆配送出错: {e}")
            future.result()


"""
          -----------------ABOVE:key algorithm functions------BELOW:assistive functions-----------------------
"""


"""
state: bug fixing
"""
# check if go home
def isBackToStart(vehicle:Vehicle, client:VehicleClient):
    pos = getPureCurrentVehiclelPos(vehicle.ID, client)
    return (np.isclose(vehicle.startPos.x, pos[0]) and np.isclose(vehicle.startPos.y, pos[1]))


"""
state: bug fixing
"""
# check if vehicle is inside an area
def isInsideArea(pos:list, area):
    if area.shape.nZoneType in [0, 3]:
        x, y = pos[0], pos[1]
        polygon = area.shape.vertices
        n = len(polygon)
        inside = False
        x_intersections = 0
        for i in range(n):
            x1, y1 = polygon[i]
            x2, y2 = polygon[(i + 1) % n]

            if min(y1, y2) < y <= max(y1, y2):
                x_intersect = x1 if x1 == x2 else x1 + (y - y1) * (x2 - x1) / (y2 - y1)
                if x_intersect >= x:
                    x_intersections += 1

        inside = (x_intersections % 2 == 1)
    elif area.shape.nZoneType == 1:
        inside = (VehicleClient.get_distance_by_lon_lat(Position(pos[0], pos[1]), Position(area.shape.center[0], area.shape.center[1])) < area.shape.radius)
    
    return inside


"""
state: bug fixing
"""
# check if a number is inside a range
def numInside(a, b, c):
    return (a>b and a<c) or (a<b and a>c)


"""
state: bug fixing
"""
# refresh the content of goods in pick and delivery areas
def pickUpGoods(vehicle:Vehicle, pickArea:Pick, pkg, client:VehicleClient):
    if pkg is None:
        return False
    
    goodsID = pkg.ID
    for goods in pickArea.content:
        if goods.ID == goodsID:
            vehicle.addContent(goods)
            pickArea.pickOutContent(goodsID)
            #pickup = False
            pickup = client.pick_up_goods(vehicle.ID, goods.ID)
            return pickup
    # while not pickup:
    #     try:
    #         if pickup:
    #             break
    #     except Exception:
    #         continue

def sendInGoods(vehicle:Vehicle, deliveryArea:Delivery, pkg, client:VehicleClient):
    if pkg is None:
        return False
    # removePkgIDList = []
    # for tempPkg in vehicle.content:
    #     if tempPkg.destinationID == deliveryArea.ID:
    #         removePkgIDList.append(tempPkg.ID)
    deliveryArea.sendInContent(pkg)
            #deliver = False
    deliver = client.delivery_goods(vehicle.ID, pkg.ID, deliveryArea.ID)
            # while not deliver:
            #     try:
            #         if deliver:
            #             break
            #     except Exception:
            #         continue
        
    vehicle.removeContent(pkg.ID)
    return deliver


"""
state: bug fixing
"""
# get specific 2D position of a vehicle, replacing the annoying get_vehicle_pos()
def getPureCurrentVehiclelPos(vehicleId, client:VehicleClient):
    for id, pos in client.get_vehicle_pos().items():
        if id == vehicleId:
            return [pos.x, pos.y]


"""
state:bug fixing
"""
# trasform list and position
def listToPosition(posList:list):
    return Position(posList[0], posList[1])


"""
state:bug fixing
"""
# check if two lines cross each other
def isCross(line1:Line, line2:Line):
    def cross(vec1, vec2):
        return vec1[0]*vec2[1] - vec1[1]*vec2[0]
    
    ax, ay, bx, by, cx, cy, dx, dy = line1.startPos[0], line1.startPos[1], line1.endPos[0], line1.endPos[1], line2.startPos[0], line2.startPos[1], line2.endPos[0], line2.endPos[1]
    ab = [bx-ax, by-ay]
    cd = [dx-cx, dy-cy]
    ad = [dx-ax, dy-ay]
    ac = [cx-ax, cy-ay]
    cb = [bx-cx, by-cy]
    ca = [ax-cx, ay-cy]
    return cross(ab, ad) * cross(ab, ac) < 0 and cross(cd, cb) * cross(cd, ca) < 0


"""
state:bug fixing
"""
# check if a point in on a line
def isOnLine(point:list, line:Line):
    x1, y1, x3, y3 = line.startPos[0], line.startPos[1], line.endPos[0], line.endPos[1]
    x2, y2 = point[0], point[1]
    if numInside(x2, x1, x3) and numInside(y2, y1, y3):
        return (y1 - y2) * (x3 - x2) == (y3 - y2) * (x1 - x2)


"""
state:bug fixing
"""
# find the max y or x value in a list of dots, serving for Conv()
def findYMax(data):
    ploy = np.array(data)
    y = ploy[:,1]
    yMax = y.max()
    ConvList = list()
    for p in data:
        if p[1] == yMax:
            ConvList.append(p)
    
    if len(ConvList) != 1:
        tempList = []
        for p in ConvList:
            tempList.append(p[0])
        return ConvList[tempList.index(max(tempList))]
    else:
        return ConvList[0]
    

"""
state:bug fixing
"""
# find the concex dot in a concave polygon
def Conv(data):
    normDot = findYMax(data)
    num = len(data)
    Ind = data.index(normDot)
    convexDots = []
    VecA = [data[Ind%num][0] - data[(Ind-1)%num][0], data[Ind%num][1] - data[(Ind-1)%num][1]]
    VecB = [data[(Ind+1)%num][0] - data[Ind%num][0], data[(Ind+1)%num][1] - data[Ind%num][1]]
    VectNorm = (VecA[0] * VecB[1]) - (VecA[1] * VecB[0])

    for i in range(num):
        VA = [data[(i)%num][0] - data[(i-1)%num][0], data[(i)%num][1] - data[(i-1)%num][1]]
        VB = [data[(i+1)%num][0] - data[(i)%num][0], data[(i+1)%num][1] - data[(i)%num][1]]
        VecCross = (VA[0] * VB[1]) - (VA[1] * VB[0])
        if (VecCross * VectNorm) < 0:
            convexDots.append(data[(i) % num])
    
    return convexDots

"""
state:bug fixing
"""
# transform meter to lontitude and latitude, angel is the sin of the angle between the direction of "distance" and north
# as for transforming lon and lat to meter, use VehicleClient.get_distance_by_lon_lat()
def meterToLatLon(distance:float, startPos:list[float], sinAngle:float, cosAngle:float):
    R = 6379000.0
    lat_origin = startPos[1]
    lon_origin = startPos[0]
    dy_north = distance * cosAngle
    dx_east = distance * sinAngle
    lat_rad_origin = math.radians(lat_origin)

    delta_lat_rad = dy_north / R
    lat_dest_rad = math.radians(lat_origin) + delta_lat_rad
    lat_dest = math.degrees(lat_dest_rad)

    delta_lon_rad = dx_east / (R * math.cos(lat_rad_origin))
    lon_dest_rad = math.radians(lon_origin) + delta_lon_rad
    lon_dest = math.degrees(lon_dest_rad)

    return [lon_dest, lat_dest]


"""
state:bug fixing
"""
def calculateDistance(node1:list[float, float], node2:list[float, float]):
    """
    计算两个节点之间的经纬坐标距离
    """
    return math.sqrt((node1[0] - node2[0])**2 + (node1[1] - node2[1])**2)


"""
state:bug fixing
"""
def calculate_real_speed(vehicle:Vehicle, client:VehicleClient) -> int:
    """
    返回水平速度大小
    """
    return client.get_vehicle_vel()[vehicle.ID].direct


"""
state:bug fixing
"""
def getSpeedDirection(vehicle:Vehicle, client:VehicleClient) -> float:
    """
    返回水平速度方向, 0 ~ 360, 0表示正北, 顺时针旋转
    """
    return client.get_vehicle_vel()[vehicle.ID].rate


"""
state:bug fixing
"""
def contractRect(rect):
    x_t, x_b, y_t, y_b = getXY_tb(rect)
    x_t = meterToLatLon(5.0, [x_t, 0.0], -1.0, 0.0)[0]  #负号写反了，已更正
    x_b = meterToLatLon(5.0, [x_b, 0.0], 1.0, 0.0)[0]
    y_t = meterToLatLon(5.0, [0.0, y_t], 0.0, -1.0)[1]
    y_b = meterToLatLon(5.0, [0.0, y_b], 0.0, 1.0)[1]
    return x_t, x_b, y_t, y_b


"""
state:bug fixing
"""
def getXY_tb(rect):
    """
    一次性获取x_t, x_b, y_t, y_b
    """
    vertices = rect.shape.vertices
    x_coords = [point[0] for point in vertices]
    y_coords = [point[1] for point in vertices]
    
    x_t = max(x_coords)  # 最大x值
    x_b = min(x_coords)  # 最小x值
    y_t = max(y_coords)  # 最大y值
    y_b = min(y_coords)  # 最小y值
    
    return x_t, x_b, y_t, y_b


"""
state:bug fixing
"""
def findIntersection(line1:Line, line2:Line):
    """
    寻找两条线段的交点
    """
    [x1, y1], [x2, y2] = line1.startPos, line1.endPos
    [x3, y3], [x4, y4] = line2.startPos, line2.endPos
    
    # 计算行列式
    det = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4)
    
    if det == 0:
        return None
    
    # 计算交点坐标
    t = ((x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4)) / det
    u = -((x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3)) / det
    
    if 0 <= t <= 1 and 0 <= u <= 1:
        x = x1 + t * (x2 - x1)
        y = y1 + t * (y2 - y1)
        return [x, y]
    
    return None